#ifndef NAVDATA4_H
#define NAVDATA4_H
#include "navdata.h"

typedef struct _navdata_vision_detect_4_t {
  quint16   tag;
  quint16   size;

  quint32   nb_detected;
  quint32   type[4];
  quint32   xc[4];
  quint32   yc[4];
  quint32   width[4];
  quint32   height[4];
  quint32   dist[4];
}_ATTRIBUTE_PACKED_ navdata_vision_detect_4_t;

typedef struct _navdata_unpacked_4_t {
  quint32  ardrone_state;
  qint32    vision_defined;

  navdata_demo_t             navdata_demo;
  navdata_time_t             navdata_time;
  navdata_raw_measures_t     navdata_raw_measures;
  navdata_phys_measures_t    navdata_phys_measures;
  navdata_gyros_offsets_t    navdata_gyros_offsets;
  navdata_euler_angles_t     navdata_euler_angles;
  navdata_references_t       navdata_references;
  navdata_trims_t            navdata_trims;
  navdata_rc_references_t    navdata_rc_references;
  navdata_pwm_t              navdata_pwm;
  navdata_altitude_t         navdata_altitude;
  navdata_vision_raw_t       navdata_vision_raw;
  navdata_vision_of_t        navdata_vision_of;
  navdata_vision_t           navdata_vision;
  navdata_vision_perf_t      navdata_vision_perf;
  navdata_trackers_send_t    navdata_trackers_send;
  navdata_vision_detect_4_t  navdata_vision_detect;
  navdata_watchdog_t         navdata_watchdog;
  navdata_adc_data_frame_t   navdata_adc_data_frame;
} navdata_unpacked_4_t;

static inline void ardrone_navdata_dump_unpacked_navdata(navdata_unpacked_4_t* navdata_unpacked)
{
    //qDebug() << "[NavDataHandler] DUMP: navdata_demo.vbat_flying_percentage: " << navdata_unpacked->navdata_demo.vbat_flying_percentage;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: START";
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.altitude;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.ctrl_state;
    //qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.detection_camera_rot;
    //qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.detection_camera_trans;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.detection_camera_type;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.detection_tag_index;
    //qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.drone_camera_rot;
    //qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.drone_camera_trans;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.num_frames;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.phi;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.psi;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.size;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.tag;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.theta;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.vbat_flying_percentage;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.vx;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.vy;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: " << navdata_unpacked->navdata_demo.vz;
    qDebug() << "[NavDataHandler] DUMP: navdata_demo: END";
}

static inline bool ardrone_navdata_unpack_all_4(navdata_unpacked_4_t* navdata_unpacked, navdata_t* navdata, quint32* cks)
{
    bool res = true;
    navdata_cks_t navdata_cks = { 0 };
    navdata_option_t* navdata_option_ptr;

    navdata_option_ptr = (navdata_option_t*) &navdata->options[0];

    memset(navdata_unpacked, 0, sizeof(*navdata_unpacked));

    navdata_unpacked->ardrone_state = navdata->ardrone_state;
    navdata_unpacked->vision_defined  = navdata->vision_defined;

    while (navdata_option_ptr != NULL) {
        // Check if we have a valid option
        if (navdata_option_ptr->size == 0) {
            qDebug() << "[NavDataHandler] Error: Option size is zero";
            navdata_option_ptr = NULL;
            res = false;
        } else {
            switch( navdata_option_ptr->tag ) {
            case NAVDATA_DEMO_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_demo);
                qDebug() << "[unpacking] navdata_demo";
                break;
            case NAVDATA_TIME_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_time);
                qDebug() << "[unpacking] navdata_time";
                break;
            case NAVDATA_RAW_MEASURES_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_raw_measures);
                qDebug() << "[unpacking] navdata_raw_measures";
                break;
            case NAVDATA_PHYS_MEASURES_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_phys_measures);
                qDebug() << "[unpacking] navdata_phys_measures";
                break;
            case NAVDATA_GYROS_OFFSETS_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_gyros_offsets);
                qDebug() << "[unpacking] navdata_gyros_offsets";
                break;
            case NAVDATA_EULER_ANGLES_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_euler_angles);
                qDebug() << "[unpacking] navdata_euler_angles";
                break;
            case NAVDATA_REFERENCES_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_references);
                qDebug() << "[unpacking] navdata_references";
                break;
            case NAVDATA_TRIMS_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_trims);
                qDebug() << "[unpacking] navdata_trims";
                break;
            case NAVDATA_RC_REFERENCES_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_rc_references);
                qDebug() << "[unpacking] navdata_rc_references";
                break;
            case NAVDATA_PWM_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_pwm);
                qDebug() << "[unpacking] navdata_pwm";
                break;
            case NAVDATA_ALTITUDE_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_altitude);
                qDebug() << "[unpacking] navdata_altitude";
                break;
            case NAVDATA_VISION_RAW_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_vision_raw);
                qDebug() << "[unpacking] navdata_vision_raw";
                break;
            case NAVDATA_VISION_OF_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_vision_of);
                qDebug() << "[unpacking] navdata_vision_of";
                break;
            case NAVDATA_VISION_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_vision);
                qDebug() << "[unpacking] navdata_vision";
                break;
            case NAVDATA_VISION_PERF_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_vision_perf);
                qDebug() << "[unpacking] navdata_vision_perf";
                break;
            case NAVDATA_TRACKERS_SEND_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_trackers_send);
                qDebug() << "[unpacking] navdata_trackers_send";
                break;
            case NAVDATA_VISION_DETECT_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_vision_detect);
                qDebug() << "[unpacking] navdata_vision_detect";
                break;
            case NAVDATA_WATCHDOG_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_unpacked->navdata_watchdog);
                qDebug() << "[unpacking] navdata_watchdog";
                break;
            case NAVDATA_ADC_DATA_FRAME_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr,
                        navdata_unpacked->navdata_adc_data_frame);
                qDebug() << "[unpacking] navdata_adc_data_frame";
                break;
            case NAVDATA_CKS_TAG:
                navdata_option_ptr = ardrone_navdata_unpack(
                        navdata_option_ptr, navdata_cks);
                qDebug() << "[unpacking] navdata_cks";
                *cks = navdata_cks.cks;
                navdata_option_ptr = NULL; // End of structure
                break;
            default:
                qDebug() << "[NavDataHandler] Tag "
                         << (int) (navdata_option_ptr->tag)
                         << " is not a valid navdata option tag";
                navdata_option_ptr = NULL;
                res = false;
                break;
            }
        }
    }
    //ardrone_navdata_dump_unpacked_navdata(navdata_unpacked);
    return res;
}

// define video channel
typedef enum
{
        ZAP_CHANNEL_HORI=0,
        ZAP_CHANNEL_VERT,
        ZAP_CHANNEL_LARGE_HORI_SMALL_VERT,
        ZAP_CHANNEL_LARGE_VERT_SMALL_HORI,
        ZAP_CHANNEL_NEXT
} ZAP_VIDEO_CHANNEL;

#endif // NAVDATA4_H
